#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>
#include<bits/stdc++.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZINormal;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

Eigen::Matrix4f P2_, Trv2c_;

void pcd2bin(pcl::PointCloud<PointType>::Ptr cloud, string& out_file)
{
    pcl::io::savePCDFile(out_file, *cloud);
}

void  addLabel(vector<vector<double>>& label,const tf::Transform& T,
    const  int frame_num,const int trk_id,const int cls_type){
    vector<double> label_this(6);
    label_this[0] = frame_num;
    label_this[1] = trk_id;
    label_this[2] = cls_type;
    label_this[3] = T.getOrigin().x();
    label_this[4] = T.getOrigin().y();
    label_this[5] = T.getOrigin().z();
    label.push_back(label_this);
}

void saveTrajectoryLabel(string& out_file, vector<vector<double>> label)
{
    ofstream outfile(out_file.c_str(),ios::out|ios::binary|ios::app);
    if(!outfile) cout<<"tra label error"<<endl;         

    int object_num=label.size();
    for(int i=0; i<object_num; i++){
        outfile
        <<label[i][0]<<" "<<label[i][1]<<" "<<label[i][2]<<" " //frame trk_id class_type
        <<label[i][3]<<" "<<label[i][4]<<" "<<label[i][5]<<std::endl;  //x yz
    }
    outfile.close();
}

void saveTrajectory(string& out_file,const int frame_num,const tf::Transform& T,const string& pc_path,
    const string& img_path,const double timestamp)
{
    ofstream outfile(out_file.c_str(),ios::out|ios::binary|ios::app);
    if(!outfile) cout<<"tra error"<<endl;
    outfile<<frame_num<<" "<<pc_path<<" "<<img_path
    <<" "<<T.getOrigin().x()<<" "<<T.getOrigin().y()<<" "<<T.getOrigin().z()<<" "    //xyz
           <<T.getRotation().w()<<" "<<T.getRotation().x()<<" "
           <<T.getRotation().y()<<" "<<T.getRotation().z()<<" "<<to_string(timestamp)<<std::endl;  //
    outfile.close();
}

bool judgeLivoxRange(const PointType& pt){
    double ratio = sin(0.707);
    if(pt.y-ratio*pt.x -3< 0 && pt.y+ratio*pt.x +3>0 ){
        return true;
    }else{
        return false;
    }
}

void cloudCB(const sensor_msgs::PointCloud2ConstPtr& pc_msg, const sensor_msgs::ImageConstPtr& front_image_msg,
                const nav_msgs::OdometryConstPtr& master_traj_gt_msg,  const nav_msgs::OdometryConstPtr& green_traj_gt_msg){
    ROS_INFO("cb ok");
    std::chrono::system_clock::time_point t1_, t2_;
    t1_ = std::chrono::system_clock::now();
    frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                      "%06d", frame_num_));
    cout<<frame_id_string_<<endl;
    pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
    pcl::fromROSMsg(*pc_msg, *pc_tmp);

    tf::Transform T_mas,T_black,T_green,T_mas2black,T_mas2green,T_lidar2base,T_cam2lidar,center_temp,T_obect,T_person1,T_person2,T_person3
                                ,T_mas2person1,T_mas2person2,T_mas2person3;
    T_mas.setOrigin(tf::Vector3(master_traj_gt_msg->pose.pose.position.x, master_traj_gt_msg->pose.pose.position.y, master_traj_gt_msg->pose.pose.position.z));
    T_mas.setRotation(tf::Quaternion(master_traj_gt_msg->pose.pose.orientation.x,master_traj_gt_msg->pose.pose.orientation.y,
                                     master_traj_gt_msg->pose.pose.orientation.z,master_traj_gt_msg->pose.pose.orientation.w));
    T_green.setOrigin(tf::Vector3(green_traj_gt_msg->pose.pose.position.x, green_traj_gt_msg->pose.pose.position.y, green_traj_gt_msg->pose.pose.position.z));
    T_green.setRotation(tf::Quaternion(green_traj_gt_msg->pose.pose.orientation.x,green_traj_gt_msg->pose.pose.orientation.y,
                                       green_traj_gt_msg->pose.pose.orientation.z,green_traj_gt_msg->pose.pose.orientation.w));
    
    T_lidar2base.setOrigin(tf::Vector3(0, 0,-1.094)); //is ground or base_link?
    T_lidar2base.setRotation(tf::Quaternion(0,0,0,1));

    double hwl_car[3];
    int point_num_threhold_=0;
    hwl_car[0]=0.85;hwl_car[1]=0.5;hwl_car[2]=0.75;
    tf::Transform T_bottom_center_car,T_bottom_center_person;
    T_bottom_center_car.setOrigin(tf::Vector3(0, 0, hwl_car[0]/2));
    T_bottom_center_car.setRotation(tf::Quaternion(0,0,0,1));

    T_mas = T_mas*T_lidar2base.inverse();
    T_green=T_green*T_bottom_center_car;

    vector<vector<double>> label;
    addLabel(label,T_green,frame_num_,0,1);

    string main_path = "/home/st/ubuntu_data/dataset/gazebo/envalute/";
//    string tranin_or_test="my_data/training";
    string dataset_name = "0927_5";
    string tranin_or_test="testing";
    cout<<tranin_or_test<<endl;
   string tra_label_path_name_=main_path+dataset_name+"/"+tranin_or_test+"/trajectory/"+"tracking_label.txt";
   saveTrajectoryLabel(tra_label_path_name_,label);

   string pc_path_name=main_path+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".pcd";
   pcl::PointCloud<PointType>::Ptr p_det(new pcl::PointCloud<PointType>);
    for (uint i = 1; i < pc_tmp->points.size(); i++)
    {
        if ((pc_tmp->points[i].x>0&&pc_tmp->points[i].x<20) && 
            (pc_tmp->points[i].y>-10&&pc_tmp->points[i].y<10)&&
            (pc_tmp->points[i].z>-5&&pc_tmp->points[i].z<5)&&
            judgeLivoxRange(pc_tmp->points[i]))
        {
            p_det->push_back(pc_tmp->points[i]);
        }
    }
   cout<<"pc points size "<<p_det->size()<<endl;
   pcd2bin(p_det,pc_path_name);

   cv_bridge::CvImagePtr image_front;
   image_front = cv_bridge::toCvCopy(front_image_msg, sensor_msgs::image_encodings::BGR8);
   string img_path_name = main_path+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png";
   cv::imwrite(img_path_name,(*image_front).image);

    string tra_path_name=main_path+dataset_name+"/"+tranin_or_test+"/trajectory/master.txt";
    saveTrajectory(tra_path_name,frame_num_,T_mas,pc_path_name,img_path_name,pc_msg->header.stamp.toSec());

    frame_num_++;
    t2_ = std::chrono::system_clock::now();
    cout << "cost time   " << std::chrono::duration_cast<std::chrono::microseconds>( t2_-t1_ ).count()*0.001<<" ms" << std::endl;
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;

    P2_ = Eigen::Matrix4f::Identity();
    P2_ <<319.9988245765257, 0, 320.5, 0,
                                 0, 319.9988245765257, 240.5, 0, 
                                 0, 0, 1, 0,
                                 0,0,0,1;

    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/robot1/Ruby80", 1);
    message_filters::Subscriber<sensor_msgs::Image> image_front_sub(nh, "/robot1/front_camera/image_raw", 1);
    message_filters::Subscriber<nav_msgs::Odometry> master_traj_sub(nh, "/robot1/base_pose_ground_truth", 1);
    message_filters::Subscriber<nav_msgs::Odometry> green_traj_sub(nh, "/nexus0/base_pose_ground_truth", 1);
//    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::PointCloud2> sync(image_left_sub, image_right_sub,pc_sub, 10);
//    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3));
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,sensor_msgs::Image, 
        nav_msgs::Odometry,nav_msgs::Odometry> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), pc_sub, image_front_sub, 
        master_traj_sub, green_traj_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.05));
    ROS_INFO("ok1");
    sync.registerCallback(boost::bind(&cloudCB, _1, _2,_3,_4));
    ROS_INFO("ok2");
    ros::spin();

    return 0;
}
